/*
dyp-rd.c
offer dyp rd supersonic sensor module driver function realize
designed by lunanting

-------------------------
2020-07-13 19:13:36
create file
-------------------------
*/


#include "dyp-rd.h"
#include "../../bsp/xp_uart.h"
#include "../../bsp/cli_cmd.h"
#include "aos/kernel.h"
#include "../../algorithm/kalman.h"
#include "stdlib.h"


static DYP_class dyp={0};
static KALMAN_class *kalman;
static int log=0;

static u8 xp_dyp_sum_check(u8 *data);
static int xp_dyp_debug(char *type,char *fun,char *param);
static u16 xp_dyp_distance(u8 h,u8 l);

/*
dyp-rd module init
param:
    port:uart port
    speed：uart speed
return；
    0=success
*/
int xp_dyp_init(u8 port,int speed){
    uart_dev_t *uart=&dyp.uart;

    uart->port=port;
    uart->config.baud_rate=speed;
    uart->config.data_width=DATA_WIDTH_8BIT;
    uart->config.parity=NO_PARITY;
    uart->config.stop_bits=STOP_BITS_1;
    uart->config.flow_control=FLOW_CONTROL_DISABLED;
    uart->config.mode=MODE_TX_RX;

    if(hal_uart_init(uart)!=0){
        println("dyp module uart init fail~");
        return -1;
    }
    void xp_dyp_recive_thread(void *arg);
    if(aos_task_new("dyp recive thread",xp_dyp_recive_thread,NULL,1024)!=0){
        println("dyp module init create dyp recive thread fail~");
        return -2;
    }
    if(xp_cli_debug_add(xp_dyp_debug)!=0){
        perr("add debug fail~");
        return -3;
    }
    kalman=xp_kalman_init(0.3,0.3);
    dyp.init=1;
    println("dyp module init success,uart port=%d,speed=%d",port,speed);
    return 0;
}



/*
recive module data thread
*/
void xp_dyp_recive_thread(void *arg){
    int cnt=0;
	u8 send=0x01;
    u16 dis=0;

    while(dyp.init!=1){
        cnt++;
        if(cnt>10){
            perr("dyp long time no init~");
            aos_task_exit(0);
        }
        aos_msleep(1000);
    }
    while(true){
        if(log==0){
            aos_msleep(500);
            continue;
        }
        memset(dyp.rec_buf,0,dyp.rec_len);
        xp_uart_rec_clear(&dyp.uart);
        hal_uart_send(&dyp.uart,&send,1,5);
        hal_uart_recv_II(&dyp.uart,dyp.rec_buf,4,&dyp.rec_len,100);
        if(xp_dyp_sum_check(dyp.rec_buf)!=0){
            dyp.dis=0;
            perr("dyp sensor no response,port=%d",dyp.uart.port);
            aos_msleep(2000);
            continue;
        }
        dis=xp_dyp_distance(dyp.rec_buf[1],dyp.rec_buf[2]);
        dyp.dis=xp_kalman_filter(kalman,dis);
        if(log!=0){
            println("dyp sensor dis = %d mm",dyp.dis);
        }
        aos_msleep(200);
    }
}

/*
dyp module data analysis
param:

*/




/*
deal dyp module recive data api weah define
param:
    data:recive data
    len:recive length
*/
void __weak xp_dyp_recive_deal_api(u8 data,u8 len){

}


/*
sensor sum check
param:
    data:check data
return:
    0=success ==
*/
static u8 xp_dyp_sum_check(u8 *data){
    u8 sum=0;
    if(data[0]!=0xff)return -1;
    sum=data[0]+data[1]+data[2];
    sum=sum&0x00ff;
    if(sum==data[3]){
        return 0;
    }
    else{
        return -1;
    }
}


/*
sensor value
param:
    h:data h
    l:data l
return: distance value
*/
static u16 xp_dyp_distance(u8 h,u8 l){
    return (h<<8)+l;
}


/*
module dis get
return: dis
*/
int xp_dyp_dis_get(void){
    return dyp.dis;
}




/************************************debug*********************************/
//dyp debug
static int xp_dyp_debug(char *type,char *fun,char *param){
    float f=0;
    if(strcmp(type,"dyp")!=0)return 0;
    else if(strcmp(fun,"Q")==0){
        f=atof(param);
        kalman->Q=f;
    }
    else if(strcmp(fun,"R")==0){
        f=atof(param);
        kalman->R=f;
    }
    else if(strcmp(fun,"log")==0){
        if(strcmp(param,"open")==0){
            log=1;
        }
        else if(strcmp(param,"close")==0){
            log=0;
        }
        else{
            println(" set fault state !!");
        }
    }

    else{
        return 0;
    }
    return 1;
}


//end of the file
